#ifndef PATH_H
#define PATH_H

#include <ros/ros.h>

#include "map/occupancy_grid.h"
#include "target_tag.h"
#include <set>
#include <map>
#include <list>
#include <queue>

typedef struct {
	float direction;
	float distance;
} PathDirections;

struct CellNode {
	int x;
	int y;
	int cost;
};

struct Waypoint {
	float x;
	float y;
};

class CostCellCompare
{
public:
	CostCellCompare(const bool& revparam=false) {}

	bool operator() (const CellNode& lhs, const CellNode&rhs) const
	{
		return (lhs.cost > rhs.cost);
	}
};

class OrderCellCompare
{
	public:
	bool operator() (const CellNode& n1, const CellNode& n2)
	{
		if (n1.x < n2.x) return true;
		if (n1.x == n2.x) return n1.y < n2.y;
		return false;
	}
};

class Path {
public:
	Path();
	~Path();

	void setCurrentPosition(float x, float y, float theta);
	void setOccupancyGrid(OccupancyGrid *grid);
	void setTags(std::vector<TargetTag *> *tags) { tags_ = tags; }
	void setTarget(TargetTag *tag);

	void renderInflatedMap();
	void renderPath();

	void updatePath();

	Waypoint getFirstWaypoint();
	int getNumWaypoints();
	std::vector<Waypoint> getWaypoints() { return waypoints_; }

	void update();
	void updateRegion(int lowerX, int lowerY, int upperX, int upperY);
	void inflateRegion(int lowerX, int lowerY, int upperX, int upperY);
private:
	float x_;
	float y_;
	float theta_;

	float robotDiameter_;

	int width_;
	int height_;
	float resolution_;

	OccupancyGrid *grid_;
	std::vector<TargetTag *> *tags_;
	TargetTag *targetTag_;

	char **cells_;
	char **cellsInflated_;
	char **inflateRegion_;
	int inflateRegionSize_;

	std::set<CellNode, OrderCellCompare> closedCells_;
	std::set<CellNode, OrderCellCompare> openCells_;
	std::priority_queue<CellNode, std::vector<CellNode>, CostCellCompare> openCellsQueue_;
	std::map<CellNode, CellNode, OrderCellCompare> parentCells_;
	std::map<CellNode, int, OrderCellCompare> gScore_;
	std::map<CellNode, int, OrderCellCompare> fScore_;

	std::list<CellNode> currentPath_;
	std::vector<Waypoint> waypoints_;

	ros::Publisher map_publisher_;
	ros::Publisher path_publisher_;

	CellNode findCellClosestToTag(TargetTag *tag);
	void search(int fromX, int fromY, TargetTag *tag);
	void updatePath(CellNode current, CellNode goal);
};

#endif
